#include <iostream>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/io/pcd_io.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/console/time.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/search/kdtree.h>
#include <pcl/search/search.h>
#include <pcl/segmentation/extract_clusters.h>
#include <vector>
#include <pcl/filters/project_inliers.h>
#include <cmath>

int main (int argc, char** argv)
{

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_down(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::PCDReader reader;
  reader.read(argv[1], *cloud);

  pcl::console::TicToc alltime;
  alltime.tic();
  pcl::console::TicToc tt0;
  tt0.tic();

  // Create the filtering object
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0, 1.0);
  pass.filter (*cloud_filtered);

  std::cout << "[passthrought, " << tt0.toc() << " ms ]" << std::endl;//time

  pcl::console::TicToc tt1;
  tt1.tic();


  //down extract
  pcl::VoxelGrid<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud_filtered);
  sor.setLeafSize (0.01f, 0.01f, 0.01f);
  sor.filter(*cloud_down);

  std::cout << "[voxelgrid, " << tt1.toc() << " ms ]" << std::endl;//time
  pcl::console::TicToc tt2;

  tt2.tic();

  //seg the plane
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.01);
  seg.setInputCloud (cloud_down);
  seg.segment (*inliers, *coefficients);

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud (cloud_down);
  extract.setIndices (inliers);
  extract.setNegative (true);
  extract.filter (*cloud_f);

  pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
  // build the filter
  outrem.setInputCloud(cloud_f);
  outrem.setRadiusSearch(0.12);//0.05  0.15 0.12
  outrem.setMinNeighborsInRadius (110);//7  80 60
  // apply filter
  outrem.filter (*cloud_filtered);

  std::cout << "[seg plane, " << tt2.toc() << " ms ]" << std::endl;//time

  pcl::console::TicToc tt3;
  tt3.tic();

  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.012); // 2cm
  ec.setMinClusterSize (290);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);

  int j = 0;
  pcl::PCDWriter writer;
  std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
  for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
    cloud_cluster->points.push_back (cloud_filtered->points[*pit]); 
  cloud_cluster->width = cloud_cluster->points.size ();
  cloud_cluster->height = 1;
  cloud_cluster->is_dense = true;

  std::cout << "[euclideancluster, " << tt3.toc() << " ms ]" << std::endl;//time

  pcl::console::TicToc tt4;
  tt4.tic();

  //seg the plane
  pcl::SACSegmentation<pcl::PointXYZ> seg1;
  pcl::PointIndices::Ptr inliers1 (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients1 (new pcl::ModelCoefficients);
  seg1.setOptimizeCoefficients (true);
  seg1.setModelType (pcl::SACMODEL_PLANE);
  seg1.setMethodType (pcl::SAC_RANSAC);
  seg1.setMaxIterations (100);
  seg1.setDistanceThreshold (0.003);
  seg1.setInputCloud (cloud_cluster);
  seg1.segment (*inliers1, *coefficients1);

  //mapping the box
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ProjectInliers<pcl::PointXYZ> proj;
  proj.setModelType (pcl::SACMODEL_PLANE);
  proj.setInputCloud (cloud_cluster);
  proj.setModelCoefficients (coefficients1);
  proj.filter (*cloud_projected);

  std::cout << "[seg box plane, " << tt4.toc() << " ms ]" << std::endl;//time

  pcl::PointXYZ min;
  pcl::PointXYZ max;
  pcl::getMinMax3D(*cloud_projected,min,max);

  std::cout << "the center_x of the box is, " << (min.x+max.x)/2  << std::endl;
  std::cout << "the angle of the box is, " << acos(coefficients1->values[2]/(sqrt(coefficients1->values[0]*coefficients1->values[0]+coefficients1->values[2]*coefficients1->values[2])))*180.0/3.1415 << std::endl;

  double x = (min.x+max.x)/2.0;
  double z = (min.z+max.z)/2.0;
  std::cout << "the distance to the origin is, " << sqrt(x*x+z*z) << std::endl;
  
  std::cout << "[all time need, " << alltime.toc() << " ms ]" << std::endl;//time

  pcl::visualization::CloudViewer viewer ("Cluster viewer");
  viewer.showCloud(cloud_projected);
  while (!viewer.wasStopped ())
  {
  }
  return (0);
}













/*
//region growing segmentation
  pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> > (new pcl::search::KdTree<pcl::PointXYZ>);
  pcl::PointCloud <pcl::Normal>::Ptr normals (new pcl::PointCloud <pcl::Normal>);
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
  normal_estimator.setSearchMethod (tree);
  normal_estimator.setInputCloud (cloud_filtered);
  normal_estimator.setKSearch (50);
  normal_estimator.compute (*normals);

  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
  reg.setMinClusterSize(50);
  reg.setMaxClusterSize(1000000);
  reg.setSearchMethod (tree);
  reg.setNumberOfNeighbours (10);
  reg.setInputCloud (cloud_filtered);
  //reg.setIndices (indices);
  reg.setInputNormals (normals);
  reg.setSmoothnessThreshold (8.0 / 180.0 * M_PI);
  reg.setCurvatureThreshold (1.0);

  std::vector <pcl::PointIndices> clusters;
  reg.extract (clusters);

  pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();
  pcl::visualization::CloudViewer viewer ("Cluster viewer");
  viewer.showCloud(colored_cloud);

  pcl::visualization::CloudViewer viewer ("Cluster viewer");
  viewer.showCloud(cloud_filtered);
  while (!viewer.wasStopped ())
  {
  }
*/

